#include "cbs_solver.h"
#include "multi_agent_solver.h"

#include <memory>
#include <vector>

namespace application {
namespace pathfinding {

struct CbsSolver::Impl {
    MultiAgentSolver mas;
};

void CbsSolver::buildFromGraph(PathfindingGraphPtr graph)
{
    impl_ = std::make_shared<Impl>();
    if (!graph) return;
    impl_->mas.buildFromGraph(graph);
}

bool CbsSolver::planSingleAgent(const QString& startId,
                                const QString& goalId,
                                QVector<QString>& route) const
{
    route.clear();
    if (!impl_ || startId.isEmpty() || goalId.isEmpty()) return false;

    AgentRequest req;
    req.agvNumber = 0;
    req.startId = startId;
    req.goalId = goalId;

    std::vector<AgentRequest> requests{ req };
    std::vector<AgentPlan> plans;
    if (!impl_->mas.solve(requests, plans, 1.0)) {
        return false;
    }
    if (plans.empty()) return false;

    route = plans.front().route;
    if (route.isEmpty() || route.back() != goalId) {
        route.append(goalId);
    }
    return true;
}

} // namespace pathfinding
} // namespace application

